#include "motor_ctrl.h"
/**
 * VESC YBI-5008电机: 
 *      定义顺时针为电机(位置、速度、扭矩)的正值增长方向
 * 参数：
 *      顺时针 为 位置 增大方向，
 *      顺时针 为 速度 增大方向，
 *      顺时针 为 扭矩 增大方向，
 * 注意：
 *      无位置速度环(混合)模式
 *      电机反馈的扭矩始终是负值,这里就将其转为绝对值， 且要转子卡住情况下，反馈的值与设定的值相等
 *      运动学正方向为逆时针
 *      位置、速度、扭矩模式有1s超时
 * */ 

Motor_VESC::Motor_VESC(Motor_ID_Info &info) : Motor(info)
{   
    std::cout<<"Motor_VESC"<<std::endl;
    this->accept_err        = 0.2f;
    this->each_angle_max    = 5.0f;
    this->cmd_freq          = VESC_FREQ;
    this->dir               = DIR_CLOCKWISE;
}

Motor_VESC::~Motor_VESC()
{
    std::cout<<"~Motor_VESC"<<std::endl;
}

int32_t Motor_VESC:: set_origin(uint8_t mode)
{  
    return 0;
}
                                       
int32_t Motor_VESC:: set_motor_pos(double pos)
{
//正负5圈
#define POS_MIN       (-1800.0f)    //5*360
#define POS_MAX       (1800.0f)
    if((pos >= POS_MIN) && (pos <= POS_MAX)) {
        this->pos = pos;
        this->cnt_round = (((int)(pos-offset))/360);
        return (int32_t)((((int)pos)%360) * 1000000);
    } else {
        this->pos = 0;
        return 0;
    }   
    return 0;
}

int32_t Motor_VESC:: set_motor_w(double w)
{
    this->w = w;
    return (int32_t)(w/6.0f*VESC_RATIO*VESC_POLES);
} 
int32_t Motor_VESC:: set_motor_t(double t)
{
    double ia = (t/VESC_T_CONST/VESC_RATIO);
    this->t = t;
    return (int32_t)(ia * 1000);

} 
int32_t Motor_VESC:: set_motor_acc(double acc)
{
    return 0;
}

int32_t Motor_VESC:: set_motor_kp(double kp)
{
    return 0;
}  
int32_t Motor_VESC:: set_motor_kd(double kd)
{
    return 0;
}

void Motor_VESC::set_cur_tau(uint8_t data[])
{
    int16_t cur_int = ((data[0] <<8)| data[1]);
    this->cur_tau =  abs(cur_int)/10.0f*VESC_T_CONST*VESC_RATIO;

}

void Motor_VESC::set_cur_pos(uint8_t data[])
{
    int16_t pos;
    pos = ((data[0] << 8) | data[1]); 
    this->cur_pos = pos/50.0f+(this->cnt_round*360);

}
void Motor_VESC::set_cur_w(uint8_t data[])
{
    int32_t w = (data[0]<<24)|(data[1]<<16)|(data[2]<<8)|(data[3]);
    this->cur_w = (w/VESC_RATIO/VESC_POLES*6.0f);   
}

int32_t Motor_VESC::set_motor_dataframe(uint8_t dataframe[], Motor_Mode mode, double value[], uint8_t len)
{
    int32_t data_len = 0;
    int32_t val;
    switch (mode) {
        case MOTOR_MODE_ENTER:
        case MOTOR_MODE_EXIT:
        case MOTOR_MODE_ORIGIN:
            return 0;
        case MOTOR_MODE_T:
            if(1 > len) {
                cout << "MOTOR_TYPE_VESC MOTOR_MODE_T len error!" << endl;
                return -1;
            }
            val = set_motor_t(value[0]);
            dataframe[0] =  (val>>24) & 0xff;
            dataframe[1] =  (val>>16) & 0xff;
            dataframe[2] =  (val>>8) & 0xff;
            dataframe[3] =  val & 0xff;
            data_len = 4;
            break;
        case MOTOR_MODE_W:
            if(1 > len) {
                cout << "MOTOR_TYPE_VESC MOTOR_MODE_W len error!" << endl;
                return -1;
            }
            val = set_motor_w(value[0]);
            dataframe[0] =  (val>>24) & 0xff;
            dataframe[1] =  (val>>16) & 0xff;
            dataframe[2] =  (val>>8) & 0xff;
            dataframe[3] =  val & 0xff;
            data_len = 4;
            break;
        case MOTOR_MODE_POS_HOLD:
        case MOTOR_MODE_POS_TRACK:
        case MOTOR_MODE_MIX:
            if(1 > len) {
                cout << "MOTOR_TYPE_VESC MOTOR_MODE_POS len error!" << endl;
                return -1;
            }
            val = set_motor_pos(value[0]);
            dataframe[0] =  (val>>24) & 0xff;
            dataframe[1] =  (val>>16) & 0xff;
            dataframe[2] =  (val>>8) & 0xff;
            dataframe[3] =  val & 0xff;
            data_len = 4;
            break;
        default:
            break;
    }
#if 0
    printf("can_send:");
    for(uint8_t i =0;i<data_len;i++) {
        printf("%02x ",dataframe[i]);
    }
    printf("\n");
#endif
    return data_len;
}

uint8_t Motor_VESC::get_motor_mode_map(Motor_Mode mode)
{
    uint8_t ret=0;
    switch(mode) {
        case MOTOR_MODE_ENTER:
        case MOTOR_MODE_EXIT:
        case MOTOR_MODE_ORIGIN:
            ret = 0;      //无
            break; 
        case MOTOR_MODE_W:
            ret = 3;
            break;
        case MOTOR_MODE_POS_HOLD:
        case MOTOR_MODE_POS_TRACK:
        case MOTOR_MODE_MIX:
            ret = 4;
            break;
        case MOTOR_MODE_T:
            ret = 1;
            break;
        default:
            break;
    }
    return ret;
} 
